# 与 /uav_control_main 节点进行交互 将规划好的路径进行坐标转换 控制无人机轨迹
import rospy
from geometry_msgs.msg import Point
from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState, UAVSetup
from prometheus_msgs.srv import *
from nav_msgs.msg import Odometry
import re
from math import fabs
from mavros_msgs.srv import WaypointPush
from mavros_msgs.msg import Waypoint, WaypointList
import mavros_msgs.msg
from geographiclib.geodesic import Geodesic

# 创建无人机相关数据变量
uav_control_state_sv = UAVControlState()
point_list = WaypointList()
uav_command_pv = UAVCommand()
uav_state_sv = UAVState()
target_pose = Odometry()
target = Point()

# 坐标转换分量
geod = Geodesic.WGS84

def targetPoseInput():
    global target
    target_pose_str = input("请输入目标点坐标(x,y,z):")
    if target_pose_str == '':
        rospy.loginfo("输入为空,请重新输入")
        targetPoseInput()
    # 使用spilt()函数将字符串转换为列表，以[,，]作为分隔符
    target_pose_list = re.split(r'[,，]', target_pose_str)    
    target.x = float(target_pose_list[0])
    target.y = float(target_pose_list[1])
    target.z = float(target_pose_list[2])

# 无人机状态回调函数
def uavStateCb(msg):
    global uav_state_sv
    uav_state_sv = msg

# 无人机控制状态回调函数
def uavControlStateCb(msg):
    global uav_control_state_sv
    uav_control_state_sv = msg

def targetPoseCb(msg):
    global target_pose
    target_pose = msg

def pathPlanClient():
    rospy.wait_for_service('path_generator')
    try:
        path_plan = rospy.ServiceProxy('path_generator', PathPlan)
        resp1 = path_plan(target)
        return resp1.path
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e)

def main():
    targetPoseInput()
    cnt = 0   
    rospy.init_node('path_plan_py',anonymous=True)
    # 创建命令发布标志位,命令发布则为true;初始化为false
    cmd_pub_flag = False
    # 无人机名称
    uav_name = rospy.get_param('uav_name', default='uav1')
    rospy.Subscriber(f"/{uav_name}/prometheus/control_state", UAVControlState, uavControlStateCb)
    rospy.Subscriber(f"/{uav_name}/prometheus/state", UAVState, uavStateCb)
    # 目标点订阅
    rospy.Subscriber(f"/{uav_name}/target_point", Odometry, targetPoseCb)
    # 无人机控制状态发布器
    uav_command_pub = rospy.Publisher(f"/{uav_name}/prometheus/control_command", UAVCommand, queue_size=10)
    # 无人机状态设置发布者
    uav_setup_pub = rospy.Publisher(f"/{uav_name}/prometheus/setup", UAVSetup, queue_size=10)
    start_lat = 47.397742
    start_lon = 8.545594
    print(f"start_lat:{start_lat}, start_lon:{start_lon}")
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 输入目标点
        if uav_control_state_sv.control_state != UAVControlState.COMMAND_CONTROL:
            rospy.loginfo("Please switch the UAV to COMMAND_CONTROL mode")
            uav_setup = UAVSetup()
            # 如果无人机未解锁则解锁
            if not uav_state_sv.armed:
                uav_setup.cmd = UAVSetup.ARMING
                uav_setup.arming = True
            else:
                uav_setup.cmd = UAVSetup.SET_CONTROL_MODE
                uav_setup.control_state = "COMMAND_CONTROL"
            uav_setup_pub.publish(uav_setup)
        if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL:
            # 将航点转化成无人机控制指令
            path_real = pathPlanClient()
            cnt += 1
            print(f"cnt:{cnt},path:{path_real}")
            if not cmd_pub_flag:
                # BUG:无人机没有按照指定的航点飞行
                uav_command_pv.header.stamp = rospy.Time.now()
                # 坐标系
                uav_command_pv.header.frame_id = 'ENU'
                # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度
                uav_command_pv.Agent_CMD = 1
                # 发布的命令ID,每发一次,该ID加1
                uav_command_pv.Command_ID = 1
                # 发布起飞命令
                uav_command_pub.publish(uav_command_pv)
                rate.sleep()
                # 命令发布标志位置为true
                cmd_pub_flag = True
                # 打印无人机起飞相关信息
                rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height'))
            else:
                if fabs(uav_state_sv.position[0] - path_real[-1].x) <= 0.1 and fabs(uav_state_sv.position[1] - path_real[-1].y) <= 0.1:
                    rospy.loginfo("Target_point reached!")
                    rospy.sleep(30)
                    uav_command_pv.Agent_CMD = UAVCommand.Land
                    uav_command_pv.Command_ID += 1
                    uav_command_pub.publish(uav_command_pv)
                    rospy.loginfo("Landing!")
                    rospy.sleep(10)
                    rospy.loginfo("Mission completed!")
                    # 任务结束,等待下一个任务发布
                if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1:
                    rospy.loginfo('UAV takeoff successfully and move ENU coordinate system!')
                    waypoint_push_client = rospy.ServiceProxy(f"/{uav_name}/mavros/mission/push", WaypointPush)
                    waypoints = [
                        Waypoint(
                            frame=mavros_msgs.msg.Waypoint.FRAME_GLOBAL_REL_ALT,
                            command=mavros_msgs.msg.CommandCode.NAV_WAYPOINT,
                            is_current=True,
                            autocontinue=True,
                            x_lat=geod.Direct(start_lat, start_lon, path_real[0].x, path_real[0].y)['lat2'],
                            y_long=geod.Direct(start_lat, start_lon, path_real[0].x, path_real[0].y)['lon2'],
                            z_alt=path_real[0].z,
                        ),
                    ]
                    for i in range(1, len(path_real)):
                        gps = geod.Direct(start_lat, start_lon, path_real[i].x, path_real[i].y)
                        print(gps)
                        print(path_real[i])
                        waypoint_temp = Waypoint()
                        waypoint_temp.frame = mavros_msgs.msg.Waypoint.FRAME_GLOBAL_REL_ALT
                        waypoint_temp.command = mavros_msgs.msg.CommandCode.NAV_WAYPOINT
                        waypoint_temp.is_current = False
                        waypoint_temp.autocontinue = True
                        waypoint_temp.x_lat = gps['lat2']
                        waypoint_temp.y_long = gps['lon2']
                        waypoint_temp.z_alt = path_real[i].z
                        waypoints.append(waypoint_temp)
                    # print(waypoints)
                    response = waypoint_push_client(0, waypoints)
                    if response.success:
                        rospy.loginfo("Waypoints uploaded to the drone")
                    else:
                        rospy.loginfo("Waypoint upload failed:" + response.wp_transfer_error)
                else:
                    rospy.loginfo(f"UAV position555:{uav_state_sv.position}")
                    rospy.sleep(1)
        else:
            if cmd_pub_flag:
                rospy.logfatal(" Unknown error! [path plan] path_plan aborted")
            else:
                rospy.logwarn("Waiting for the UAV to enter the [COMMAND_CONTROL] state")
                rospy.sleep(1)
        rate.sleep()

    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass